#include "head3_solver.h"


int main() {
    ThetaSolver solver(0.275, 0.335, 0.150);
    solver.set_parameters(0.490772, -0.846193, -0.207606,
                          0.398544, 0.255645, 0.248709);
    // std::vector<double> lb = {0, 0, 0};
    // std::vector<double> ub = {2 * M_PI, 2 * M_PI, 2 * M_PI};
    
    std::vector<double> lb = {-M_PI_2, 40.0/180.0*M_PI, 40.0/180.0*M_PI};
    std::vector<double> ub = { M_PI_2, 150.0/180.0*M_PI, 170.0/180.0*M_PI};
    auto solutions = solver.solve_all(lb,ub,50);

    int count = 1;
    for (const auto& theta : solutions) {
        std::cout << "解 #" << count++ << ":\n";
        std::cout << "theta1 = " << theta[0] << " rad (" << theta[0] * 180.0 / M_PI << " deg)\n";
        std::cout << "theta2 = " << theta[1] << " rad (" << theta[1] * 180.0 / M_PI << " deg)\n";
        std::cout << "theta3 = " << theta[2] << " rad (" << theta[2] * 180.0 / M_PI << " deg)\n\n";
    }

    std::cout << "共找到 " << solutions.size() << " 个唯一解。\n";
    return 0;
}
